#include "calibrationdialog.h"
#include "ui_calibrationdialog.h"

#include <Kinect/kinectframe.h>
#include <Kinect/kinectsync.h>
#include <libfreenect_sync.h>
#include <libfreenect.h>
#include <QDebug>
using namespace kinect;

CalibrationDialog::CalibrationDialog(QWidget *parent) :
    QDialog(parent),
    ui(new Ui::CalibrationDialog)
{
    count=0;
    ui->setupUi(this);
     ui->bad->setEnabled(false);
}

CalibrationDialog::~CalibrationDialog()
{
    delete ui;
}

void CalibrationDialog::on_getFrame_clicked()
{
    ui->bad->setEnabled(false);
    char* rgb=0;
    char* ir=0;
    uint32_t ts,ts2;
    cv::Mat rgbMat;
    cv::Mat irMat;
    irMat.create(cv::Size(FREENECT_IR_FRAME_W,FREENECT_IR_FRAME_H),CV_8UC1);
    rgbMat.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_8UC3);


    qDebug() << "Get IR";
    freenect_sync_get_video((void**)&ir,  &ts, 0, FREENECT_VIDEO_IR_8BIT);
    qDebug() << "IR memcpy";
    memcpy(irMat.data,ir,FREENECT_VIDEO_IR_8BIT_SIZE);
    qDebug() << "Get RGB";

    freenect_sync_get_video((void**)&rgb,  &ts2, 0, FREENECT_VIDEO_RGB);
    qDebug() << "Time lapce=" << ts2-ts;
    memcpy(rgbMat.data,rgb,FREENECT_VIDEO_RGB_SIZE);

    Mat lr,rr;
    if(calib.addImage(irMat,rgbMat,lr,rr)){
        qDebug() << "Success";
        ui->view->showPair(lr,rr);
        count++;
        ui->count->display(count);
        ui->bad->setEnabled(true);
    }else{
        qDebug() << "Fail";
    }
}

void CalibrationDialog::on_bad_clicked()
{
    count--;
    ui->count->display(count);
    ui->bad->setEnabled(false);
    calib.removeLast();
}

void CalibrationDialog::on_Calibrate_clicked()
{
    if(calib.callibrate())
    {
            qDebug() << "Calibrated!!!";
            calib.saveSettings("ololo.xml");
            calib.getCalib(SterioCalibration::LEFT).saveDebug("debug_left.xml");
            calib.getCalib(SterioCalibration::RIGHT).saveDebug("debug_right.xml");
    }else{
            calib.loadSettings("ololo.xml");
    }

    KinectFrame kf=grabFrame(0);
    kinect::KinectSync ks;
    ks.assignDeviceCallibration(0,calib);
    ks.transform(kf);
    Mat newrgb = ks.transform2(kf);
    imwrite("test.bmp",newrgb);
}

KinectFrame  CalibrationDialog::grabFrame(int id)
{
    Mat rgb,depth;
    //Buffers init
    rgb.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_8UC3);
    depth.create(cv::Size(FREENECT_FRAME_W,FREENECT_FRAME_H),CV_16UC1);
  //  Mat tmp(cv::Size(FREENECT_IR_FRAME_W,FREENECT_IR_FRAME_H),CV_16UC1);

    uint32_t ts;
    char *t_rgb = 0;
    short *t_depth=0;
    double dx,dy,dz;

    freenect_sync_get_depth((void**)&t_depth, &ts, id, FREENECT_DEPTH_11BIT);
    freenect_sync_get_video((void**)&t_rgb,&ts,id,FREENECT_VIDEO_RGB);

    if(0==t_rgb||0==t_depth)
    {
        return KinectFrame();
    }

    freenect_raw_tilt_state* state;



    freenect_sync_get_tilt_state(&state,id);
    freenect_get_mks_accel(state,&dx,&dy,&dz);
    cv::Point3f pos = cv::Point3f(dx,dy,dz);
    memcpy(rgb.data,t_rgb,FREENECT_VIDEO_RGB_SIZE);
    memcpy(depth.data,t_depth,FREENECT_DEPTH_11BIT_SIZE);
    return KinectFrame(id,rgb,depth,pos);
}
